% Advanced PID control project
% 3DOPF Hover Labortory
% parameter setting
% l:(m)the distance form the prop cneter to the pivot point

l = 0.1969

% Jp:(kg.m^2)the moment of interia about the pitch axis
Jp = 0.0552

% Jr:(kg.m^2)the moment of interia about the roll axis
Jr = 0.0552

% Jy:(kg.m^2)the moment of interia about the yaw axis
Jy = Jp + Jr

% Kf:(N/V) is the force constant and is equal to the force generated form
% the fans per unit voltage into the amplifier
Kf = 0.594

% Kt:(Nm/V) the torque aopnstant and is equal to the torque generated by
% the fans ( in the body yaw) poer unit voltage into the amplifier
Kt = 0.018

Am = [0 1 0 0 0 0; 0 0 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 0 0;
    0 0 0 0 0 1;0 0 0 0 0 0]

Bm = [0 0 0 0 ; -(Kt/Jy) -(Kt/Jy) (Kt/Jy) (Kt/Jy);
    0 0 0 0 ; l*Kf/Jp -l*Kf/Jp 0 0 ; 0 0 0 0;
    0 0 l*Kf/Jp -l*Kf/Jp]

Cm = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0]

Dm=zeros(3,4)

hover_model=ss(Am,Bm,Cm,Dm);
disp('Open loop poles are')
Pole(hover_model)
disp('Open loop poles are all at 0, the system only responses to a very low freq since the roll off rate is -120db/decade at the beginning');
disp(' ');
disp(' ');


disp('Question 2')
disp('Rank number is')
rank(ctrb(hover_model))
disp('Cond number is')
cond(ctrb(hover_model))
disp('full rank:controlable. cond number is reasenablly small, system can be easily controled')
disp(' ');
disp(' ');

disp('Question 3')
Cm
disp(' ');
disp(' ');

disp('Question 4')
disp('Rank number is')
rank(obsv(hover_model))
disp('Cond number is')
cond(obsv(hover_model))
disp('full rank:observable. cond number is 1, system is ideally observable')
disp(' ');
disp(' ');

disp('Question 5')
Am_agm=[Am zeros(6,3);...
    1 0 0 0 0 0 0 0 0;...
    0 0 1 0 0 0 0 0 0;...
    0 0 0 0 1 0 0 0 0];
Bm_agm=[Bm;zeros(3,4)];
Cm_agm=[Cm zeros(3)];
Dm_agm=Dm;
hover_model_agm=ss(Am_agm,Bm_agm,Cm_agm,Dm_agm);

Q=diag([10000 100 20000 500 20000 500 20000 20000 20000]);
R=eye(4);

K=lqr(Am_agm,Bm_agm,Q,R)
Km=K(:,1:6)
Ki=K(:,7:9)
disp(' ');
disp(' ');


disp('Question 6')
disp('The CL system poles are')
CLpole=eig(Am-Bm*Km)
disp(' ');
disp(' ');


disp('Question 7')
Mat_in=[zeros(6,3);eye(3)]
Am_fb_agm=[Am-Bm*Km -Bm*Ki;...
          Cm-Dm*Km -Dm*Ki];
Bm_fb_agm=Mat_in;
Cm_fb_agm=Cm_agm;
Dm_fb_agm=zeros(3);

hover_fb_agm=ss(Am_fb_agm,Bm_fb_agm,Cm_fb_agm,Dm_fb_agm)
disp(' ');
disp(' ');


disp('Question 8')
disp('Desired CL poles of hover_fb_agm')
P_desired=10*pole(hover_fb_agm)

disp('Observer gain is')
L=transpose(place(Am',Cm',CLpole))

TF=tf(hover_model)
TFp=TF(2,1)*2/5
TFy=TF(1,4)*4/5


% design the PID controller
% employ the pole placement method
% the T1 & T2 are infinite

% the Petch and roll PID controller
wp=0.6;   % natural frequency omega
Zp=0.4;   % damping ratio
ap=2.5;   % real pole
Ap=0.8475;
b=1
c=1
N=20

Kpp=wp^2*(1+2*Zp*ap)/Ap
Tip=(1+2*Zp*ap)/(ap*wp)
Tdp=(ap+2*Zp)/(wp*(1+2*Zp*ap))

Gspp=Kpp*(b+tf([1],[Tip 0])+c*tf([Tdp 0],[Tdp/N 1]));
Gcp=Kpp*(1+tf([1],[Tip 0])+tf([Tdp 0],[Tdp/N 1]));
Hp=feedback(TFp*Gcp,1);
Hphat=minreal((TFp*Gspp)/(1+TFp*Gcp));

display('Gspp = ');Gspp
display('Gcp = ');Gcp
display('Hp = ');Hp
display('Hphat = ');Hphat

% the Yaw PID controller
wy=0.18;
Zy=0.7;
ay=2.2;
Ay=0.1304;

Kpy=wy^2*(1+2*Zy*ay)/Ay
Tiy=(1+2*Zy*ay)/(ay*wy)
Tdy=(ay+2*Zy)/(wy*(1+2*Zy*ay))

Gspy=Kpy*(b+tf([1],[Tiy 0])+c*tf([Tdy 0],[Tdy/N 1]));
Gcy=Kpy*(1+tf([1],[Tiy 0])+tf([Tdy 0],[Tdy/N 1]));
Hy=feedback(TFy*Gcy,1);
Hyhat=minreal((TFy*Gspy)/(1+Gcy*TFy));
display('Gspy = ');Gspy
display('Gcy = ');Gcy
display('Hy = ');Hy
display('Hyhat = ');Hyhat

% Double PID controler
beta = 0.25

% generate a ITAE robust PID controller
wn=10;
k1=0.8475;
k2=0.1304;

Kpip=2.15*wn^2/k1;
Kiip=wn^3/k1;
Kdip=1.75*wn/k1;

Gcip=tf([Kdip Kpip Kiip],[1 0])
Titaep=feedback(TFp*Gcip,1)


Kpiy=2.15*wn^2/k2;
Kiiy=wn^3/k2;
Kdiy=1.75*wn/k2;

Gciy=tf([Kdiy Kpiy Kiiy],[1 0])
Titaey=feedback(TFy*Gciy,1)

Am = [0,1;0,0];
Bm = [0,0,0,0;1,-1,0,0];
Cm = [1,0];